#include <Arduino.h>
#include <Wire.h>
#include "Adafruit_VL53L0X.h"
#include "main.h"
#include "assitant.h"
#include "jcs_vl53l0x.h"

int JCSensor_Vl53l0x::ins_count = 0;
Adafruit_VL53L0X *JCSensor_Vl53l0x::__lox = NULL;
JCSensor_Vl53l0x::~JCSensor_Vl53l0x()
{
    if (!lox)
    {
        return;
    }
    delete lox;
    ins_count--;
    if (!ins_count)
        __lox = NULL;
}
int JCSensor_Vl53l0x::setup(String &config)
{
    lox = new Adafruit_VL53L0X();
    if (!lox)
    {
        return -1;
    }
    if (!ins_count)
        Wire.begin(SD3, SD2);
    if (!lox->begin())
    {
        xouts(F("Failed to boot VL53L0X"));
        delete lox;
        lox = NULL;
        return -2;
    }
    __lox = lox;
    if (!ins_count)
    {
        if (search_fun_in_Lily_ui("tof") < 0)
        {
            public_a_fun_link_int("tof", (void *)_fun_tof, 0);
#if __vl53l0x_enable_alert
            _monitor_setup();
#endif
        }
    }
    ins_count++;
    return 0;
}
int JCSensor_Vl53l0x::read_data(Adafruit_VL53L0X *_lox)
{
    VL53L0X_RangingMeasurementData_t measure;
    _lox->rangingTest(&measure, false);
    if (measure.RangeStatus != 4)
    {
        return measure.RangeMilliMeter;
    }
    return -1;
}
int JCSensor_Vl53l0x::_fun_tof()
{
    if (__lox)
        return -__LINE__;
    VL53L0X_RangingMeasurementData_t measure;
    __lox->rangingTest(&measure, false);
    if (measure.RangeStatus == 4)
    {
        return -__LINE__;
    }
    int val = measure.RangeMilliMeter;
    xouts(val);
    return val;
}
String JCSensor_Vl53l0x::read()
{
    int val = read_data(lox);
    if (val < 0)
    {
        return String();
    }
    sprintf(tx_back, "\"tof\":%d", val);
    String res(tx_back);
    return res;
}

#pragma region Alert
#if __vl53l0x_enable_alert
#define _vlx_monitor_fre 14

bool JCSensor_Vl53l0x::_vlx_monitor_on = false;
_VLX_State JCSensor_Vl53l0x::_vlx_state = WaitForTrigger;
long int JCSensor_Vl53l0x::_vlx_last_enter_time = 0;
Pipe JCSensor_Vl53l0x::_vlx_monitor_trigger_pipe = NULL;
int JCSensor_Vl53l0x::_lox_warning_threshold = 250;  // 25cm
int JCSensor_Vl53l0x::_lox_warning_threshold2 = 100; // 10cm
int JCSensor_Vl53l0x::_lox_holding_time = 500;       // ms
int JCSensor_Vl53l0x::_lox_letting_time = 10000;     // 10s

int JCSensor_Vl53l0x::_vlx_task_monitor()
{
    if (!_vlx_monitor_on)
        return 0;
    if (!__lox)
    {
        _fun_lox_monitor(0);
    }
    int val = read_data(__lox);
    bool _detected = False;
    if (val > _lox_warning_threshold2 && val < _lox_warning_threshold)
    {
        _detected = true;
    }
    unsigned long int _now = 0;
#define NextTo(state) _vlx_state = state;
    switch (_vlx_state)
    {
    case WaitForTrigger:
        if (!_detected) // not enter
            return 0;
        _vlx_last_enter_time = millis();
        NextTo(DuringTrigger);
        break;
    case DuringTrigger: // wait for holding 0.5s
        if (!_detected)
        {
            NextTo(WaitForTrigger);
            return 0;
        }
        _now = millis();
        if (_now - _vlx_last_enter_time < _lox_holding_time)
        {
            return 0;
        }
        // do action
        NextTo(OnAction);
        break;
    case OnAction:
        if (_detected)
        {
            return 0;
        }
        _now = millis();
        if (_now - _vlx_last_enter_time > _lox_letting_time) // exceed the maximum time
        {
            NextTo(WaitForTrigger);
            return 0;
        }
        _vlx_monitor_on = false;
        if (_vlx_monitor_trigger_pipe)
        {
            char tx[8];
            itoa(val, tx, 10);
            tx[7] = 0;
            pipe_release_data(_vlx_monitor_trigger_pipe, tx, strlen(tx));
        }
        int _short_beep();
        _short_beep();
        remove_timer(_vlx_task_monitor);
        add_timer_once([]
                       {
                            if (!had_timer(_vlx_task_monitor))
                                public_a_timer(_vlx_task_monitor,Hz(_vlx_monitor_fre));
                            NextTo(WaitForTrigger);
                            _vlx_monitor_on = true;
                            return 0; },
                       Second(5));
        break;

    case WaitLeft:
        if (!_detected)
            NextTo(WaitForTrigger);
        break;
    default:
        break;
    }
    return 0;
#undef NextTo
}

int JCSensor_Vl53l0x::_monitor_setup()
{
    public_a_var_ref("max_lx", &_lox_warning_threshold, 'd');
    public_a_var_ref("min_lx", &_lox_warning_threshold2, 'd');
    public_a_var_ref("lx_hd", &_lox_holding_time, 'd');
    public_a_var_ref("lx_lt", &_lox_letting_time, 'd');
    public_a_fun_link_int("lx_monitor", (void *)_fun_lox_monitor, 1);
    _vlx_monitor_trigger_pipe = new_input_device("vxp");
    return 0;
}

int JCSensor_Vl53l0x::_fun_lox_monitor(int mode)
{
    if (mode)
    {
        if (!__lox)
        {
            li_error("no sensor loaded", -__LINE__);
        }
        if (!had_timer(_vlx_task_monitor))
            public_a_timer(_vlx_task_monitor, Hz(_vlx_monitor_fre));
        outs("lvx monitor started");
        _vlx_monitor_on = true;
        return 0;
    }
    _vlx_monitor_on = false;
    remove_timer(_vlx_task_monitor);
    outs("lvx monitor stoped");
    return 0;
}
#endif
#pragma endregion
